package com.btlib;

import com.btlib.lib.CarControl;
import com.btlib.lib.MyActivity;
import android.os.Bundle;
import android.os.Handler;
import android.os.Message;
import android.util.Log;
import android.view.View;
import android.view.View.OnClickListener;
import android.widget.Button;

public class RunCircle extends MyActivity{
  private CarControl cc;
  private static final String TAG = "RunCircle";
  
  @Override
  protected void onCreate(Bundle savedInstanceState) {
    // TODO Auto-generated method stub
    super.onCreate(savedInstanceState);
    setContentView(R.layout.main);
    
    cc = new CarControl(this);
    Button btn_start = (Button) findViewById(R.id.btn_start);
    btn_start.setOnClickListener(new OnClickListener(){
      public void onClick(View v){
        cc.controlStart();
      }
    });
    Button btn_disconn = (Button) findViewById(R.id.btn_disconn);
    btn_disconn.setOnClickListener(new OnClickListener(){
      @Override
      public void onClick(View v) {
        cc.controlDisconnect();
        //connected = false;
        //firstRun = true;
      }
    });
  }
  
  public void distanceDetect(){
    //cc.controlStop();
    cc.controlSensor((byte)(-90));
    statemach.distanceRight = cc.getDistance();
    cc.controlSensor((byte)90);
    statemach.distanceFoward = cc.getDistance();
    cc.controlSensor((byte)90);
    statemach.distanceLeft = cc.getDistance();
    cc.controlSensor((byte)(-90));
  }
  public void orientTurn(int dir){
    switch(dir){
    case StateMachine.FORWARD:
      cc.controlFoward(CarControl.DEFSPEED);
      break;
    case StateMachine.LEFT:
      cc.controlLeft(CarControl.DEFSPEED);
      break;
    case StateMachine.RIGHT:
      cc.controlRight(CarControl.DEFSPEED);
      break;
    }
  }
  private static final int ENDCONN = 100;
  private StateMachine statemach = new StateMachine();
  
  private Handler mHandler = new Handler(){
    public void handleMessage(Message msg) {
      switch(msg.what){
      case ENDCONN:
        //connected = true;
        //do something
        Log.e(TAG, "connected");
        statemach.control();
        break;
      }
    }
  };

  @Override
  public void endConnect() {
    // TODO Auto-generated method stub
    mHandler.sendEmptyMessage(ENDCONN);
  }
  
  private class StateMachine{
    public int distanceFoward;
    public int distanceLeft;
    public int distanceRight;
    public static final int DISTHRESHOLD = 50;
    public static final int FORWARD = 0;
    public static final int LEFT = 1;
    public static final int RIGHT = -1;
    
    /*width of the veranda*/
    public static final int widthVeranda = 200;
    /*width of the gate*/
    public static final int widthGate = 93;
    private final int S1 = 0, S2 = 1, S3 = 2;
    private final int C1 = 0, C2 = 1, C3 = 2, C4 = 3;
    private int curCarState = C1, curSensorState = S1;
    private final int RT = 5000;
    public void control(){
      new Thread(new Runnable(){
        @Override
        public void run() {
          cc.controlFoward(CarControl.DEFSPEED);
          while(true){
            try{
              if(curCarState == C1 && curSensorState == S1){
                cc.controlFoward(CarControl.DEFSPEED);
                Thread.sleep(1000);
                distanceFoward = cc.getDistance();
                stateSwitch();
              }
              if(curCarState == C2 && curSensorState == S1){
                cc.controlStop();
                distanceDetect();
                stateSwitch();
              }
              if(curCarState == C3 && curSensorState == S3){
                cc.controlLeft(CarControl.DEFSPEED);
                cc.controlFoward(CarControl.DEFSPEED);
                Thread.sleep(RT);
                stateSwitch();
              }
              if(curCarState == C2 && curSensorState == S3){
                cc.controlStop();
                distanceRight = cc.getDistance();
                stateSwitch();
              }
              if(curCarState == C4 && curSensorState == S2){
                cc.controlRight(CarControl.DEFSPEED);
                cc.controlFoward(CarControl.DEFSPEED);
                Thread.sleep(RT);
                stateSwitch();
              }
              if(curCarState == C2 && curSensorState == S2){
                cc.controlStop();
                distanceLeft = cc.getDistance();
                stateSwitch();
              }
              if(curCarState == C3 && curSensorState == S1){
                cc.controlLeft(CarControl.DEFSPEED);
                cc.controlFoward(CarControl.DEFSPEED);
                stateSwitch();
              }
              if(curCarState == C4 && curSensorState == S1){
                cc.controlRight(CarControl.DEFSPEED);
                cc.controlFoward(CarControl.DEFSPEED);
                stateSwitch();
              }
            }
            catch (InterruptedException e) {
              e.printStackTrace();
            }
          }
        }
      }).start();
    }
    public void stateSwitch(){
      if(curCarState == C1 && curSensorState == S1){
        if(distanceFoward < DISTHRESHOLD)
          curCarState = C2;
        return;
      }
      if(curCarState == C2 && curSensorState == S1){
        if(distanceLeft > distanceRight){
          if(distanceRight < widthVeranda - widthGate){
            curCarState = C3;
            cc.controlSensor((byte)-90);
            curSensorState = S3;
            return;
          }
          else{
            curCarState = C4;
            cc.controlSensor((byte)90);
            curSensorState = S2;
            return;
          }
        }
        else if(distanceLeft < distanceRight){
          if(distanceLeft < widthVeranda - widthGate){
            curCarState = C3;
            cc.controlSensor((byte)-90);
            curSensorState = S3;
            return;
          }
          else{
            curCarState = C4;
            cc.controlSensor((byte)90);
            curSensorState = S2;
            return;
          }
        }
        else{//distanceLeft == distanceRight
          
        }
      }
      if(curCarState == C3 && curSensorState == S3){
        curCarState = C2;
        curSensorState = S3;
        return;
      }
      if(curCarState == C4 && curSensorState == S2){
        curCarState = C2;
        curSensorState = S2;
        return;
      }
      if(curCarState == C2 && curSensorState == S3){
        if(distanceRight > widthVeranda){
          curCarState = C4;
          cc.controlSensor((byte)90);
          curSensorState = S1;
          return;
        }
        else{
          curCarState = C1;
          cc.controlSensor((byte)90);
          curSensorState = S1;
          return;
        }
      }
      if(curCarState == C2 && curSensorState == S2){
        if(distanceLeft > widthVeranda){
          curCarState = C3;
          cc.controlSensor((byte)-90);
          curSensorState = S1;
          return;
        }
        else{
          curCarState = C1;
          cc.controlSensor((byte)-90);
          curSensorState = S1;
          return;
        }
      }
      if(curCarState == C4 && curSensorState == S1){
        curCarState = C1;
        return;
      }
      
      if(curCarState == C3 && curSensorState == S1){
        curCarState = C1;
        return;
      }
    }
  }
}
